# Single Color RGB565 Blob Tracking Example
#
# This example shows off single color RGB565 tracking using the OpenMV Cam.

import sensor, image, time, math
import pyb
from pyb import UART
import ustruct

threshold_index = 0 # 0 for red, 1 for green, 2 for blue

# Color Tracking Thresholds (L Min, L Max, A Min, A Max, B Min, B Max)
# The below thresholds track in general red/green/blue things. You may wish to tune them...
thresholds = [(29, 54, 43, 80, 13, 53), # generic_red_thresholds
              (30, 100, -64, -8, -32, 32), # generic_green_thresholds
              (0, 30, 0, 64, -128, 0)] # generic_blue_thresholds

x_max = 320
x_min = 0
x_1 = 135   #中心区域左边界
x_2 = 175   #中心区域右边界

y_max = 240
y_min = 0
y_1 = 110   #中心区域上边界
y_2 = 130   #中心区域下边界
flag = 0    #位置信息标志

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)  #240×320像素
sensor.skip_frames(time = 10)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()
#sensor.set_vflip(True)
uart = UART(3, 115200)  #串口3初始化，波特率115200

# Only blobs that with more pixels than "pixel_threshold" and more area than "area_threshold" are
# returned by "find_blobs" below. Change "pixels_threshold" and "area_threshold" if you change the
# camera resolution. "merge=True" merges all overlapping blobs in the image.

def find_max(blobs):
    max_size=0
    for blob in blobs:
        if blob.pixels() > max_size:
            max_blob=blob
            max_size = blob.pixels()
    return max_blob

def sending_data(x,y):
    data =ustruct.pack("<bbhhb",              #一个h为一个整形数据
                   0x2C,                       #帧头1
                   0x12,                       #帧头2
                   int(x), # up sample by 4    #数据1,低位在前
                   int(y), # up sample by 4    #数据2
                   #int(flag),
                   0x5B)
    uart.write(data);   #必须要传入一个字节数组
    #print(data)


while(True):
    clock.tick()
    img = sensor.snapshot()
    blobs = img.find_blobs([thresholds[threshold_index]])
    if blobs:
        max_blob=find_max(blobs)                        #找出最大的色块
        img.draw_rectangle(max_blob.rect())
        img.draw_cross(max_blob.cx(), max_blob.cy())
        #print(max_blob.cx(), max_blob.cy())


        if max_blob.cx()>= x_min  and max_blob.cx() <= 160 and\
           max_blob.cy() >= 120 and max_blob.cy() <= y_max :
                flag = 1
        if max_blob.cx()>=160 and max_blob.cx() <= x_max and\
           max_blob.cy() >=120 and max_blob.cy() <= y_max :
                flag = 2
        if max_blob.cx()>= x_min and max_blob.cx() <= 160 and \
           max_blob.cy() >= y_min and max_blob.cy() <= 120 :
                flag = 3
        if max_blob.cx()>= 160 and max_blob.cx() <= x_max and \
           max_blob.cy() >= y_min and max_blob.cy() <= 120 :
                flag = 4
        if max_blob.cx()>= x_1 and max_blob.cx() <= x_2 and \
           max_blob.cy() >= y_1 and max_blob.cy() <= y_2 :
            flag = 5

        x = max_blob.cx()
        y = max_blob.cy()
        uart.write("a\r\n")

        #sending_data(x,y);

    else:
        uart.write("b\r\n")

 #          if max_blob>0:

        #output_str="[%d,%d,%dE]" %(x,y,flag)
        #uart.write(output_str)
         #print(x,y,flag)

    #print(clock.fps())




